#include <am.h>
#include <klib.h>
#include <klib-macros.h>
#include <stdarg.h>

#if !defined(__ISA_NATIVE__) || defined(__NATIVE_USE_KLIB__)

static int locked = 0;

struct sbuf {
	char *buf;
	char *ebuf;
	int cnt;
};

static void cputch(int c, int *cnt) {
	putch(c);
	(*cnt)++;
}

static void sputch(int c, struct sbuf *b) {
	assert(b != NULL);
	b->cnt++;
	if (b->ebuf == NULL) {
		*b->buf++ = c;
	} else {
		if (b->buf < b->ebuf) {
			*b->buf++ = c;
		}
	}
}

static void printnum(void (* putc)(int c, void *data), void *data, 
	unsigned long long num, unsigned base, int width, int padc) {

	unsigned long long res = num / base;
	unsigned long mod = num % base;

	if (num >= base) {
		printnum(putc, data, res, base, width - 1, padc);
	} else {
		while (--width > 0) {
			putc(padc, data);
		}
	}

	putc("0123456789ABCDEF"[mod], data);
}

static unsigned long long getuint(va_list *ap, int lflag) {
	if (lflag >= 2) {
		return va_arg(*ap, unsigned long long);
	} else if (lflag) {
		return va_arg(*ap, unsigned long);
	} else {
		return va_arg(*ap, unsigned int);
	}
}

static long long getint(va_list *ap, int lflag) {
	if (lflag >= 2) {
		return va_arg(*ap, long long);
	} else if (lflag) {
		return va_arg(*ap, long);
	} else {
		return va_arg(*ap, int);
	}
}

void vprintfmt(void (*putc)(int c, void *data), void *data, const char *fmt, va_list ap) {
	register const char *p;
	register int ch;
	unsigned long long num;
	int base, width, precision, lflag, altflag;
	va_list ap_copy;
	va_copy(ap_copy, ap);

	while (atomic_xchg(&locked, 1) == 1) ;

	while (1) {
		while ((ch = *(unsigned char *)fmt++) != '%') {
			if (ch == '\0') {
				va_end(ap_copy);
				atomic_xchg(&locked, 0);
				return;
			}
			putc(ch, data);
		}

		char padc = ' ';
		width = precision = -1;
		lflag = altflag = 0;

	reswitch:
		switch ((ch = *(unsigned char *)fmt++)) {
			case '-':
				padc = '-';
				goto reswitch;
			
			case '0':
				padc = '0';
				goto reswitch;
			
			case '1' ... '9':
				for (precision = 0; ; ++fmt) {
					precision = precision * 10 + ch - '0';
					ch = *fmt;
					if (ch < '0' || ch > '9') {
						break;
					}
				}
				goto process_precision;

			case '.':
				if (width < 0)
					width = 0;
				goto reswitch;

			case '#':
				altflag = 1;
				goto reswitch;

			process_precision:
				if (width < 0)
					width = precision, precision = -1;
				goto reswitch;

			case 'l':
				lflag++;
				goto reswitch;

			case 'c':
				putc(va_arg(ap_copy, int), data);
				break;

			case 's':
				if ((p = va_arg(ap_copy, char *)) == NULL) {
					p = "(null)";
				}

				if (width > 0 && padc != '-') {
					for (width -= strnlen(p, precision); width > 0; width--) {
						putc(padc, data);
					}
				}

				for (; (ch = *p++) != '\0' && (precision < 0 || --precision >= 0); width--) {
					if (altflag && (ch < ' ' || ch > '~')) {
						putc('?', data);
					} else {
						putc(ch, data);
					}
				}

				for (; width > 0; width--) {
					putc(' ', data);
				}

				break;
			
			// signed decimal
			case 'd':
				num = getint(&ap_copy, lflag);
				if ((long long)num < 0) {
					putc('-', data);
					num = -(long long)num;
				}
				base = 10;
				goto number;

			// unsigned decimal
			case 'u':
				num = getuint(&ap_copy, lflag);
				base = 10;
				goto number;

			case 'o':
				num = getuint(&ap_copy, lflag);
				base = 8;

			// pointer
			case 'p':
				putc('0', data);
				putc('x', data);
				num = (unsigned long long)(uintptr_t)va_arg(ap_copy, void *);
				base = 16;
				goto number;

			// (unsigned) hexadecimal
			case 'x':
				num = getuint(&ap_copy, lflag);
				base = 16;
			number:
				printnum(putc, data, num, base, width, padc);
				break;

			// escaped '%' character
			case '%':
				putc(ch, data);
				break;

        // unrecognized escape sequence - just print it literally
			default:
				putc('%', data);
				for (fmt--; fmt[-1] != '%'; fmt--)
					/* do nothing */;
				break;
		}
	}
}

int vprintf(const char *fmt, va_list ap) {
	int cnt = 0;
	vprintfmt((void *)cputch, &cnt, fmt, ap);
	return cnt;
}

int printf(const char *fmt, ...) {
	va_list ap;
	int cnt;
	va_start(ap, fmt);
	cnt = vprintf(fmt, ap);
	va_end(ap);
  return cnt;
}

int vsprintf(char *out, const char *fmt, va_list ap) {
	struct sbuf b = { out, NULL, 0 };
	assert(out != NULL);
	vprintfmt((void *)sputch, &b, fmt, ap);
	*b.buf = '\0';
	return b.cnt;
}

int sprintf(char *out, const char *fmt, ...) {
	va_list ap;
	int cnt;
	va_start(ap, fmt);
	cnt = vsprintf(out, fmt, ap);
	va_end(ap);
	return cnt;
}

int snprintf(char *out, size_t n, const char *fmt, ...) {
	va_list ap;
	int cnt;
	va_start(ap, fmt);
	cnt = vsnprintf(out, n, fmt, ap);
	va_end(ap);
  return cnt;
}

int vsnprintf(char *out, size_t n, const char *fmt, va_list ap) {
	struct sbuf b = { out, out + n - 1, 0 };
	if (out == NULL || b.buf > b.ebuf) {
		assert(1);
	}
	vprintfmt((void *)sputch, &b, fmt, ap);
	*b.buf = '\0';

  return b.cnt;
}


#endif
